
#include <stdio.h>
#include "robot.h"
#include "keya.h"
#include "dataqueue.h"

extern Cthread_canworkqueue * cansendqueue;   //can发送队列结构体

/**
  * @brief  KEYA 设备使能控制
  * @param 
  * @retval None
  */
void KEYAActivatedModeEnable(uint32_t keyaid,KEYAACTIVE_t isactive)
{
	CANTxMsg_t txmsg;
	DATA32_t candata;
	txmsg.CANTxStruct.StdId = 0x600 + keyaid; //SDO COB-ID = 0x600 +ID;
	txmsg.CANTxStruct.DLC = 0x05;	//keya的使能使用5字节SDO的命令字 0x2F
	txmsg.CANTxStruct.Data[0] = 0x2F;	 //命令字0x22
	if(isactive == KEYADISABLE)
	{
		txmsg.CANTxStruct.Data[1] = 0x0C;  //对象名称 0x200C-00 紧急关机
		txmsg.CANTxStruct.Data[2] = 0x20; 
	}
	else
	{
		txmsg.CANTxStruct.Data[1] = 0x0D;  //对象名称 0x200C-00 紧急关机释放
		txmsg.CANTxStruct.Data[2] = 0x20; 
	}
	txmsg.CANTxStruct.Data[3] = 0x00;   //数据为0
	//通过data32类型进行转换
	candata.data32 = 0;
	txmsg.CANTxStruct.Data[4] = candata.raw[0];
	txmsg.CANTxStruct.Data[5] = candata.raw[1];
	txmsg.CANTxStruct.Data[6] = candata.raw[2]; 	
	txmsg.CANTxStruct.Data[7] = candata.raw[3]; 			
	//加入发送缓冲队列
	can_add_task(cansendqueue,&txmsg);
}

/**
  * @brief  KEYA 目标转速（rpm） 参数设置(0-1000)
  * @param 
  * @retval None
  */
void KEYATargetVelocitySet(uint32_t keyaid ,uint8_t channel, uint32_t targetvelocity)
{
	CANTxMsg_t txmsg;
	DATA32_t candata;
	txmsg.CANTxStruct.StdId = 0x600 + keyaid; //SDO COB-ID = 0x600 +ID;
	txmsg.CANTxStruct.DLC = 0x08;	//keya速度控制使用8直接SDO的命令字 0x23
	txmsg.CANTxStruct.Data[0] = 0x23;	 //命令字0x23
	txmsg.CANTxStruct.Data[1] = 0x00;  //对象名称 0x2000-00
	txmsg.CANTxStruct.Data[2] = 0x20; 

	txmsg.CANTxStruct.Data[3] = channel; //所有科亚驱动默认通道号1

	//通过data32类型进行转换
	candata.data32 = (uint32_t)targetvelocity;
	txmsg.CANTxStruct.Data[4] = candata.raw[0];
	txmsg.CANTxStruct.Data[5] = candata.raw[1];
	txmsg.CANTxStruct.Data[6] = candata.raw[2]; 	
	txmsg.CANTxStruct.Data[7] = candata.raw[3]; 			
	//加入发送缓冲队列
	can_add_task(cansendqueue,&txmsg);
}


/**
  * @brief  KEYA 目标转速（rpm） 获取
  * @param 
  * @retval None
  */
void KEYAVelocityNowGet(uint32_t keyaid ,uint8_t channel)
{
	CANTxMsg_t txmsg;
	txmsg.CANTxStruct.StdId = 0x600 + keyaid; //SDO COB-ID = 0x600 +ID;
	txmsg.CANTxStruct.DLC = 0x04;	//4字节命令字
	txmsg.CANTxStruct.Data[0] = 0x40;	 //命令字0x23
	txmsg.CANTxStruct.Data[1] = 0x03;  //对象名称 0x2103-01,02
	txmsg.CANTxStruct.Data[2] = 0x21; 
	txmsg.CANTxStruct.Data[3] = channel;  //所有科亚驱动默认通道号1
	
	//加入发送缓冲队列
	can_add_task(cansendqueue,&txmsg);
}

/**
  * @brief  KEYA 目标电流（mA） 获取
  * @param 
  * @retval None
  */
void KEYACurrentNowGet(uint32_t keyaid ,uint8_t channel)
{
	CANTxMsg_t txmsg;
	txmsg.CANTxStruct.StdId = 0x600 + keyaid; //SDO COB-ID = 0x600 +ID;
	txmsg.CANTxStruct.DLC = 0x04;	//4字节命令字
	txmsg.CANTxStruct.Data[0] = 0x40;	 //命令字0x23
	txmsg.CANTxStruct.Data[1] = 0x00;  //对象名称 0x2100-01,02
	txmsg.CANTxStruct.Data[2] = 0x21; 
	txmsg.CANTxStruct.Data[3] = channel;  //
		
	//加入发送缓冲队列
	can_add_task(cansendqueue,&txmsg);
}

/**
  * @brief  KEYA 编码器计数相对值 获取
  * @param 
  * @retval None
  */
void KEYAEncoderRelativeValueGet(uint32_t keyaid ,uint8_t channel)
{
	CANTxMsg_t txmsg;
	txmsg.CANTxStruct.StdId = 0x600 + keyaid; //SDO COB-ID = 0x600 +ID;
	txmsg.CANTxStruct.DLC = 0x04;	//4字节命令字
	txmsg.CANTxStruct.Data[0] = 0x40;	 //命令字0x23
	txmsg.CANTxStruct.Data[1] = 0x08;  //对象名称 0x2104-01,02
	txmsg.CANTxStruct.Data[2] = 0x21; 
	txmsg.CANTxStruct.Data[3] = channel; //通道号2	
	//加入发送缓冲队列
	can_add_task(cansendqueue,&txmsg);
}



/**
  * @brief  KEYA型驱动---GetCurrentNow
  * @param  None
  * @retval None
  */
void ServoUseKEYAGetCurrentNow(void *this)
{
	uint32_t keyaid;
	uint32_t channel;
	RobotServo_t *Servo = this;
	keyaid = Servo->CANID;
	channel = Servo->Channel;
	
	//调底层
	KEYACurrentNowGet(keyaid,channel); //电流获取
}

/**
  * @brief  KEYA型驱动---GetVelocityNow
  * @param  None
  * @retval None
  */
void ServoUseKEYAGetVelocityNow(void *this)
{
	uint32_t keyaid;
	uint32_t channel;
	RobotServo_t *Servo = this;
	keyaid = Servo->CANID;
	channel = Servo->Channel;
	
	//调底层
	KEYAVelocityNowGet(keyaid,channel); //速度获取
}

/**
  * @brief  KEYA型驱动---KEYAEncoderRelativeValueGet
  * @param  None
  * @retval None
  */
void ServoUseKEYAGetEncoderRelativeValueNow(void *this)
{
	uint32_t keyaid;
	uint32_t channel;
	RobotServo_t *Servo = this;
	keyaid = Servo->CANID;
	channel = Servo->Channel;
	
	//调底层
	KEYAEncoderRelativeValueGet(keyaid,channel); //编码器计数绝对值
}




void ServoUseKEYAInit(void *this)
{
	uint32_t keyaid;
	RobotServo_t *Servo = this;
	keyaid = Servo->CANID;
	
	//KEYA的初始化---当前仅仅对使能进行配置，其他配置---待续
	KEYAActivatedModeEnable(keyaid,KEYAENABLE); //开启使能
}


void ServoUseKEYAStop(void *this)
{
	uint32_t keyaid;
	RobotServo_t *Servo = this;
	keyaid = Servo->CANID;
	Servo->Status = KEYADISABLE;
	Servo->ServoVelocity = 0;
	//KEYA的初始化---当前仅仅对使能进行配置，其他配置---待续
	KEYAActivatedModeEnable(keyaid,KEYADISABLE); //开启使能
}

/**
  * @brief  KEYA型驱动---SetVelocity
  * @param  None
  * @retval None
  */
void ServoUseKEYASetVelocity(void *this)
{
	uint32_t keyaid;
	uint32_t channel;
	uint32_t velocity;
	RobotServo_t *Servo = this;
	keyaid = Servo->CANID;
	velocity = Servo->ServoVelocity;
	channel = Servo->Channel;
	//结构体里面的速度信息---由应用层函数更新 
	//速度设定函数仅仅设置驱动器速度参数，不发送 开始运动 指令
	//虽然不发送开始运动指令，但是该驱动器会在使能状态直接运动
	KEYATargetVelocitySet(keyaid, channel, velocity);
	
}
/**
  * @brief  KEYA型驱动---RUN
  * @param  None
  * @retval None
  */
void ServoUseKEYARun(void *this)
{
	uint32_t keyaid;
	uint32_t velocity;
	uint32_t mode;
	uint32_t channel;
	RobotServo_t *Servo = this;
	keyaid = Servo->CANID;
	velocity = Servo->ServoVelocity;
	channel = Servo->Channel;
	mode = Servo->CtrlMode;
	
	switch ( mode )
	{
		case PROFILEVELOCITYMODE:
			//速度控制
			KEYATargetVelocitySet(keyaid, channel, velocity);
		break;
		case PROFILEPOSITIONMODE:
			//位置控制待续-----------------------------
		
		break;
		default:
			//非法的模式，直接返回
			return;
			break;
	}
	
}